/*============================================================================*/
/*                         The Freescale Cup                                 */
/*============================================================================*/
/*                        OBJECT SPECIFICATION                                */
/*============================================================================*
* %name:            %	APPLeds.c
* %version:         %	1.0
* %created_by:      %   Continental
* %date_created:    %   Sun Aug 21 16:20:00 2011 
*=============================================================================*/
/* DESCRIPTION : Contains applications that use the TRK-MPC5604B LEDS         */
/*============================================================================*/
/* FUNCTION COMMENT : comment here										      */
/*                                                                            */
/*                                                                            */
/*============================================================================*/
/*                               OBJECT HISTORY                               */
/*============================================================================*/
/*  REVISION |   DATE      |                               |      AUTHOR      */
/*----------------------------------------------------------------------------*/
/*  1.0      | 21/08/2011  |                               | Continental     */
/* Application using LEDS to show the scheduler working                       */
/*============================================================================*/

/* Includes */
/* -------- */
#include "stdtypedef.h"
#include "APPMotors.h"


//Varibles que usaran otras funciones

//Varibles que usa de otras funciones 
extern T_UWORD ruw_PLE;//Left Motor encoder counter                 
extern T_UWORD ruw_PRE;//Right Motor encoder counter
extern T_SWORD ruw_WRM;
extern T_SWORD ruw_WLM;


/* Functions macros, constants, types and datas         */
/* ---------------------------------------------------- */
/* Functions macros */

/*==================================================*/ 
/* Definition of constants                          */
/*==================================================*/ 


/*======================================================*/ 
/* Definition of RAM variables                          */
/*======================================================*/ 


/*======================================================*/ 
/* close variable declaration sections                  */
/*======================================================*/ 

/* Private defines */

/* Private functions prototypes */
/* ---------------------------- */

/* Exported functions prototypes */
/* ----------------------------- */


/**************************************************************
*  Name                 : Led1_init
*  Description          : Prepare Led1 application.
						  This function initialize Led1_app 
*  Parameters           :  none
*  Return               :  none
*  Critical/explanation :  No
**************************************************************/
void APPMotors_LeftMotor_PID(void)
{
	static T_SWORD rsw_KpLM = 5;//25
	static T_SWORD rsw_KiLM = 10;//10
	static T_SWORD rsw_KdLM = 0;
	
  	T_SWORD rsw_ukLM; //Left Motor output control signal
	T_SWORD rsw_ekLM; //Left Motor error
	T_UWORD ruw_VelLM; //Left Motor Velocty
	T_UWORD ruw_ukLMpwm; //Left PWM out
	
	static T_SWORD rsw_ek0LM; //Error ek-1
    static T_SWORD rsw_ek1LM; //Error ek-2
    static T_SWORD rsw_uk0LM; //Salida yk-1
    
	//Left control varible
	static T_SWORD rsw_aLM; 
	static T_SWORD rsw_bLM;
	static T_SWORD rsw_cLM;
  	
  	rsw_aLM = rsw_KpLM + rsw_KiLM + rsw_KdLM;
  	rsw_bLM = (-1) * ( rsw_KpLM + (2*rsw_KdLM) );
  	rsw_cLM = rsw_KdLM;
  	
  	ruw_VelLM = (ruw_PLE);
  	ruw_VelLM = ruw_VelLM * 4; //Convercion a radianes por segundo
  	ruw_PLE = 0; //reset contador de pulsos  
  	
  	rsw_ekLM = ruw_WLM - (T_SWORD)(ruw_VelLM); //Calculo del error
  	rsw_ukLM = rsw_uk0LM + (rsw_aLM * rsw_ekLM) + (rsw_bLM * rsw_ek0LM) + (rsw_cLM * rsw_ek1LM) ; //Salida
    
    if (rsw_ukLM>=1000)
    {
    	rsw_ukLM = 1000;
    }
    
    
    if (rsw_ukLM<=(-1000))
    {
    	rsw_ukLM = -1000;
    }
    
    ruw_ukLMpwm = (T_UWORD)(rsw_ukLM + 1000); // Converter to pwm signal
       
    if(ruw_VelLM <= 8) //En caso de que choque que pare los motores
  	{
  	 ruw_ukLMpwm = 0;	
  	}
  	 
  	pwm_LeftMotor(ruw_ukLMpwm);
  	
  	//Guardar Variables
   	rsw_ek1LM = rsw_ek0LM;
   	rsw_ek0LM = rsw_ekLM;
   	rsw_uk0LM = rsw_ukLM; 
}


/**************************************************************
*  Name                 : FUNCION DEL PIN  1 para activar el driver de m1
*  Description          : Prepare Led1 application.
						  This function initialize Led1_app 
*  Parameters           :  none
*  Return               :  none
*  Critical/explanation :  No
**************************************************************/
void APPMotors_RightMotor_PID(void)
{
	static T_SWORD rsw_KpRM = 5;//25
	static T_SWORD rsw_KiRM = 10;//10
	static T_SWORD rsw_KdRM = 0;
	
  	T_SWORD rsw_ukRM; //Left Motor output control signal
	T_SWORD rsw_ekRM; //Left Motor error
	T_UWORD ruw_VelRM; //Left Motor Velocty
	T_UWORD ruw_ukRMpwm; //Left PWM out
	
	static T_SWORD rsw_ek0RM; //Error ek-1
    static T_SWORD rsw_ek1RM; //Error ek-2
    static T_SWORD rsw_uk0RM; //Salida yk-1
    
	//Left control varible
	static T_SWORD rsw_aRM; 
	static T_SWORD rsw_bRM;
	static T_SWORD rsw_cRM;
  	
  	rsw_aRM = rsw_KpRM + rsw_KiRM + rsw_KdRM;
  	rsw_bRM = (-1) * ( rsw_KpRM + (2*rsw_KdRM) );
  	rsw_cRM = rsw_KdRM;
  	
  	ruw_VelRM = (ruw_PRE);
  	ruw_VelRM = ruw_VelRM * 4; //Convercion a radianes por segundo
  	ruw_PRE = 0; //reset contador de pulsos
  	
  	rsw_ekRM = ruw_WRM - (T_SWORD)(ruw_VelRM); //Calculo del error
  	rsw_ukRM = rsw_uk0RM + (rsw_aRM * rsw_ekRM) + (rsw_bRM * rsw_ek0RM) + (rsw_cRM * rsw_ek1RM) ; //Salida
    
    if (rsw_ukRM>=1000)
    {
    	rsw_ukRM = 1000;
    }
    
    
    if (rsw_ukRM<=(-1000))
    {
    	rsw_ukRM = -1000;
    }
  	
    ruw_ukRMpwm = (T_UWORD)(rsw_ukRM + 1000); // Converter to pwm signal
    
    if(ruw_VelRM <= 8) //En caso de que choque que pare los motores
  	{
  	 ruw_ukRMpwm = 0;	
  	} 
  	
  	pwm_RightMotor(ruw_ukRMpwm);
  	
  	//Guardar Variables
   	rsw_ek1RM = rsw_ek0RM;
   	rsw_ek0RM = rsw_ekRM;
   	rsw_uk0RM = rsw_ukRM;   
}